#ifndef JPS_GRAPH_SEARCH_H
#define JPS_GRAPH_SEARCH_H

#include <memory>
#include <limits>
#include <vector>
#include <unordered_map>
#include <opencv2/opencv.hpp>
#include <boost/heap/d_ary_heap.hpp>

namespace JPS
{
  template <class T>
  struct compare_state
  {
    bool operator()(T a1, T a2) const
    {
      double f1 = a1->g + a1->h;
      double f2 = a2->g + a2->h;
      if( ( f1 >= f2 - 0.000001) && (f1 <= f2 +0.000001) )
        return a1->g < a2->g; // if equal compare gvals
      return f1 > f2;
    }
  };

  struct State;
  using StatePtr = std::shared_ptr<State>;
  using priorityQueue = boost::heap::d_ary_heap<StatePtr, boost::heap::mutable_<true>, boost::heap::arity<2>, boost::heap::compare<compare_state<StatePtr>>>;

  struct State
  {
    int id;
    int x, y, z = 0;
    int dx, dy, dz;
    int parentId = -1;
    priorityQueue::handle_type heapkey;
    double g = std::numeric_limits<double>::infinity();
    double h;
    bool opened = false;
    bool closed = false;
    State(int id, int x, int y, int dx, int dy ): id(id), x(x), y(y), dx(dx), dy(dy){}
    State(int id, int x, int y, int z, int dx, int dy, int dz ): id(id), x(x), y(y), z(z), dx(dx), dy(dy), dz(dz){}
  };

  struct JPS2DNeib {
    int ns[9][2][8];
    int f1[9][2][2];  // 需要判断障碍物栅格的个数
    int f2[9][2][2];  // 需要判断强制邻居的个数
    static constexpr int nsz[3][2] = {{8, 0}, {1, 2}, {3, 2}};

    void print();
    JPS2DNeib();
    private:
    void Neib(int dx, int dy, int norm1, int dev, int& tx, int& ty);
    void FNeib(int dx, int dy, int norm1, int dev, int& fx, int& fy, int& nx, int& ny);
  };

  class GraphSearch
  {
    public:
      GraphSearch(const cv::Mat &cMap, int xDim, int yDim, double eps = 1, bool verbose = false);
      bool plan(std::pair<int, int> xyStart, std::pair<int, int> xyGoal, bool useJps, int maxExpand = -1);

      std::vector<StatePtr> getPath() const;
      std::vector<StatePtr> getOpenSet() const;
      std::vector<StatePtr> getCloseSet() const;
      std::vector<StatePtr> getAllSet() const;

    private:
      bool plan(StatePtr& currNode_ptr, int max_expand, int start_id, int goal_id);
      void getSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs);
      void getJpsSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs);
      std::vector<StatePtr> recoverPath(StatePtr node, int id);

      int coordToId(int x, int y) const;
      bool isFree(int x, int y) const;
      bool isOccupied(int x, int y) const;
      double getHeur(int x, int y) const;
      bool hasForced(int x, int y, int dx, int dy);
      bool jump(int x, int y, int dx, int dy, int& new_x, int& new_y);

      cv::Mat cMap_;
      int xDim_, yDim_, zDim_;
      double eps_;
      bool verbose_;

      const char val_free_ = 0;
      int xGoal_, yGoal_, zGoal_;
      bool use_2d_;
      bool use_jps_ = false;

      priorityQueue pq_;
      std::vector<StatePtr> hm_;
      std::vector<bool> seen_;

      std::vector<StatePtr> path_;

      std::vector<std::vector<int>> ns_;
      std::shared_ptr<JPS2DNeib> jn2d_;
 };
}
#endif
